﻿#include "TempObstaclesNavMesh.h"

#include "TileCacheOther.cpp"
#include "../Detour/DetourCommon.h"

#include "../DetourTileCache/Include/DetourTileCache.h"
#include "../DetourTileCache/Include/DetourTileCacheBuilder.h"

#include "../Detour/DetourNavMesh.h"
#include "../Detour/DetourNavMeshQuery.h"
#include "../Detour/DetourNavMeshBuilder.h"

#include "../Contrib/fastlz/fastlz.h"
//#include <Finder/TempObstaclesNavMesh.h>
//#include <Finder/PathFinder.h>
//#include <Finder/TileCacheOther.h>
//#include <Detour/DetourCommon.h>
//#include <DetourTileCache/Include/DetourTileCache.h>
//#include <DetourTileCache/Include/DetourTileCacheBuilder.h>
//#include <Detour/DetourNavMesh.h>
//#include <Detour/DetourNavMeshQuery.h>
//#include <Detour/DetourNavMeshBuilder.h>
//#include <Contrib/fastlz/fastlz.h>

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>

#include <time.h>
#include <sstream>

#define _USE_MATH_DEFINES
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846 
#endif

static const int TILECACHESET_MAGIC = 'T' << 24 | 'S' << 16 | 'E' << 8 | 'T'; //'TSET';
static const int TILECACHESET_VERSION = 1;






#ifdef OUTPUT_DEBUG
static std::string GetTimestamp()
{
	struct tm* local_time = NULL;
	char str_time[100];
	auto begin_time = time(NULL);
	local_time = localtime(&begin_time);
	strftime(str_time, sizeof(str_time), "%Y-%m-%d:%H:%M:%S", local_time);
	return str_time;
}
#endif

template<class T> inline T rcMin(T a, T b) { return a < b ? a : b; }

template<class T> inline T rcMax(T a, T b) { return a > b ? a : b; }

inline bool inRange(const float* v1, const float* v2, const float r, const float h)
{
	const float dx = v2[0] - v1[0];
	const float dy = v2[1] - v1[1];
	const float dz = v2[2] - v1[2];
	return (dx * dx + dz * dz) < r * r && fabsf(dy) < h;
}

static bool getSteerTarget(dtNavMeshQuery* navQuery, const float* startPos, const float* endPos,
	const float minTargetDist,
	const dtPolyRef* path, const int pathSize,
	float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef,
	float* outPoints = 0, int* outPointCount = 0)
{
	// Find steer target.
	static const int MAX_STEER_POINTS = 3;
	float steerPath[MAX_STEER_POINTS * 3];
	unsigned char steerPathFlags[MAX_STEER_POINTS];
	dtPolyRef steerPathPolys[MAX_STEER_POINTS];
	int nsteerPath = 0;
	navQuery->findStraightPath(startPos, endPos, path, pathSize,
		steerPath, steerPathFlags, steerPathPolys, &nsteerPath, MAX_STEER_POINTS);
	if (!nsteerPath)
		return false;

	if (outPoints && outPointCount)
	{
		*outPointCount = nsteerPath;
		for (int i = 0; i < nsteerPath; ++i)
			dtVcopy(&outPoints[i * 3], &steerPath[i * 3]);
	}


	// Find vertex far enough to steer to.
	int ns = 0;
	while (ns < nsteerPath)
	{
		// Stop at Off-Mesh link or when point is further than slop away.
		if ((steerPathFlags[ns] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ||
			!inRange(&steerPath[ns * 3], startPos, minTargetDist, 1000.0f))
			break;
		ns++;
	}
	// Failed to find good point to steer to.
	if (ns >= nsteerPath)
		return false;

	dtVcopy(steerPos, &steerPath[ns * 3]);
	steerPos[1] = startPos[1];
	steerPosFlag = steerPathFlags[ns];
	steerPosRef = steerPathPolys[ns];

	return true;
}

static int fixupCorridor(dtPolyRef* path, const int npath, const int maxPath,
	const dtPolyRef* visited, const int nvisited)
{
	int furthestPath = -1;
	int furthestVisited = -1;

	// Find furthest common polygon.
	for (int i = npath - 1; i >= 0; --i)
	{
		bool found = false;
		for (int j = nvisited - 1; j >= 0; --j)
		{
			if (path[i] == visited[j])
			{
				furthestPath = i;
				furthestVisited = j;
				found = true;
			}
		}
		if (found)
			break;
	}

	// If no intersection found just return current path. 
	if (furthestPath == -1 || furthestVisited == -1)
		return npath;

	// Concatenate paths.	

	// Adjust beginning of the buffer to include the visited.
	const int req = nvisited - furthestVisited;
	const int orig = rcMin(furthestPath + 1, npath);
	int size = rcMax(0, npath - orig);
	if (req + size > maxPath)
		size = maxPath - req;
	if (size)
		memmove(path + req, path + orig, size * sizeof(dtPolyRef));

	// Store visited
	for (int i = 0; i < req; ++i)
		path[i] = visited[(nvisited - 1) - i];

	return req + size;
}

// This function checks if the path has a small U-turn, that is,
// a polygon further in the path is adjacent to the first polygon
// in the path. If that happens, a shortcut is taken.
// This can happen if the target (T) location is at tile boundary,
// and we're (S) approaching it parallel to the tile edge.
// The choice at the vertex can be arbitrary, 
//  +---+---+
//  |:::|:::|
//  +-S-+-T-+
//  |:::|   | <-- the step can end up in here, resulting U-turn path.
//  +---+---+
static int fixupShortcuts(dtPolyRef* path, int npath, dtNavMeshQuery* navQuery)
{
	if (npath < 3)
		return npath;

	// Get connected polygons
	static const int maxNeis = 16;
	dtPolyRef neis[maxNeis];
	int nneis = 0;

	const dtMeshTile* tile = 0;
	const dtPoly* poly = 0;
	if (dtStatusFailed(navQuery->getAttachedNavMesh()->getTileAndPolyByRef(path[0], &tile, &poly)))
		return npath;

	for (unsigned int k = poly->firstLink; k != DT_NULL_LINK; k = tile->links[k].next)
	{
		const dtLink* link = &tile->links[k];
		if (link->ref != 0)
		{
			if (nneis < maxNeis)
				neis[nneis++] = link->ref;
		}
	}

	// If any of the neighbour polygons is within the next few polygons
	// in the path, short cut to that polygon directly.
	static const int maxLookAhead = 6;
	int cut = 0;
	for (int i = dtMin(maxLookAhead, npath) - 1; i > 1 && cut == 0; i--) {
		for (int j = 0; j < nneis; j++)
		{
			if (path[i] == neis[j]) {
				cut = i;
				break;
			}
		}
	}
	if (cut > 1)
	{
		int offset = cut - 1;
		npath -= offset;
		for (int i = 1; i < npath; i++)
			path[i] = path[i + offset];
	}

	return npath;
}


TempObstaclesNavMesh::TempObstaclesNavMesh()
{
	m_navQuery = dtAllocNavMeshQuery();

	m_filter.setIncludeFlags(SAMPLE_POLYFLAGS_ALL ^ SAMPLE_POLYFLAGS_DISABLED);
	m_filter.setExcludeFlags(0);

	m_polyPickExt[0] = 2;
	m_polyPickExt[1] = 4;
	m_polyPickExt[2] = 2;

	m_talloc = new dtTileCacheAlloc();
	m_tcomp = new FastLZCompressor;
	m_tmproc = new MeshProcess;
//#ifdef OUTPUT_DEBUG
//	ofs.open("PathLibDebug.log", std::ofstream::out);
//#endif
}

TempObstaclesNavMesh::~TempObstaclesNavMesh()
{
	RemoveAllTO();
	dtFreeNavMeshQuery(m_navQuery);
	dtFreeTileCache(m_tileCache);
	delete m_navMesh;
	delete m_talloc;
	delete m_tcomp;
	delete m_tmproc;

}

bool TempObstaclesNavMesh::LoadMeshFile(std::string file_name)
{
	FILE* fp = fopen(file_name.c_str(), "rb");
	if (!fp) return false;

	// Read header.
	TileCacheSetHeader header;
	size_t headerReadReturnCode = fread(&header, sizeof(TileCacheSetHeader), 1, fp);
	if (headerReadReturnCode != 1)
	{

		// Error or early EOF
		fclose(fp);
		return false;
	}
	if (header.magic != TILECACHESET_MAGIC)
	{
		fclose(fp);
		return false;
	}
	if (header.version != TILECACHESET_VERSION)
	{
		fclose(fp);
		return false;
	}

	m_navMesh = dtAllocNavMesh();
	if (!m_navMesh)
	{
		fclose(fp);
		return false;
	}
	dtStatus status = m_navMesh->init(&header.meshParams);
	if (dtStatusFailed(status))
	{
		fclose(fp);
		return false;
	}

	m_tileCache = dtAllocTileCache();
	if (!m_tileCache)
	{
		fclose(fp);
		return false;
	}
	status = m_tileCache->init(&header.cacheParams, m_talloc, m_tcomp, m_tmproc);
	if (dtStatusFailed(status))
	{
		fclose(fp);
		return false;
	}

	// Read tiles.
	for (int i = 0; i < header.numTiles; ++i)
	{
		TileCacheTileHeader tileHeader;
		size_t tileHeaderReadReturnCode = fread(&tileHeader, sizeof(tileHeader), 1, fp);
		if (tileHeaderReadReturnCode != 1)
		{
			// Error or early EOF
			fclose(fp);
			return false;
		}
		if (!tileHeader.tileRef || !tileHeader.dataSize)
			break;

		unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize, DT_ALLOC_PERM);
		if (!data) break;
		memset(data, 0, tileHeader.dataSize);
		size_t tileDataReadReturnCode = fread(data, tileHeader.dataSize, 1, fp);
		if (tileDataReadReturnCode != 1)
		{
			// Error or early EOF
			dtFree(data);
			fclose(fp);
			return false;
		}

		dtCompressedTileRef tile = 0;
		dtStatus addTileStatus = m_tileCache->addTile(data, tileHeader.dataSize, DT_COMPRESSEDTILE_FREE_DATA, &tile);
		if (dtStatusFailed(addTileStatus))
		{
			dtFree(data);
		}

		if (tile)
			m_tileCache->buildNavMeshTile(tile, m_navMesh);
	}

	fclose(fp);
	return true;
}

bool TempObstaclesNavMesh::LoadNavMesh(std::string file_name)
{
	if (file_name=="") {
		printf("TempObstaclesNavMesh::LoadNavMesh Error File Name!\n");
		return false;
	}

	if (m_navMesh != nullptr)
	{
		dtFreeNavMesh(m_navMesh);
		m_navMesh = nullptr;
	}
	if (m_tileCache != nullptr)
	{
		dtFreeTileCache(m_tileCache);
		m_tileCache = nullptr;
	}
	if (!LoadMeshFile(file_name))
	{
		return false;
	}

	dtStatus result = m_navQuery->init(m_navMesh, 2048);

	if (dtStatusFailed(result))
	{
		return false;
	}
#ifdef OUTPUT_DEBUG
	ofs << GetTimestamp() << " TempObstaclesNavMesh::LoadNavMesh upToDate " << file_name << " " << this << std::endl;
#endif
	return true;
	
}

void TempObstaclesNavMesh::FindPath(float sp[3], float ep[3])
{
#ifdef OUTPUT_DEBUG
	ofs << GetTimestamp() << " TempObstaclesNavMesh::FindPath sp " 
		<< sp[0] << " " << sp[1] << " " << sp[2] << " "
		<< ep[0] << " " << ep[1] << " " << ep[2] << " "
		<< this << std::endl;
#endif
	for (int i = 0; i < 3; i++) {
		m_spos[i] = sp[i];
		m_epos[i] = ep[i];
	}

	auto sFindResult = m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
	if (!dtStatusSucceed(sFindResult))
	{
		printf("TempObstaclesNavMesh::FindPath fail, %ud\n", sFindResult);
		return;
	}
	auto eFindResult = m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);
	if (!dtStatusSucceed(eFindResult))
	{
		printf("TempObstaclesNavMesh::FindPath fail, %ud\n", eFindResult);
		return;
	}

	auto findResult = m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
	//for (int i = 0; i < m_npolys; i++) {
	//	printf("poly:%d ", m_polys[i]);
	//}
	//printf("\n");
	if (!dtStatusSucceed(findResult))
	{
		printf("TempObstaclesNavMesh::FindPath fail, %ud\n", findResult);
		return;
	}

	/*int m_npolys = 0;
	int m_nstraightPath = 0;*/
	m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
	//printf("PathFinder::Find m_navMesh m_npolys = %ld \n", m_npolys);
	if (m_npolys)
	{
		float epos[3];
		dtVcopy(epos, m_epos);
		if (m_polys[m_npolys - 1] != m_endRef)
			m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos, 0);

		m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
			m_Path, m_PathFlags,
			m_PathPolys, &m_nstraightPath, MAX_POLYS, 0);
		if (m_nstraightPath)
		{
			//printf("PathFinder::Find m_nstraightPath %ld\n", m_nstraightPath);
			//for (int i = 0; i < m_nstraightPath; i++)
			//{
			//	printf("PathFinder::Find m_nstraightPath %f %f %f\n", m_Path[i * 3], m_Path[i * 3 + 1], m_Path[i * 3 + 2]);
			//}
			return ;
		}
	}
}

void TempObstaclesNavMesh::Update()
{
	if (!m_navMesh)
		return;
	if (!m_tileCache)
		return;

	// 这个调用次数可能和帧率有关
	// 202106 CE的第二号场景，在开始的时候创建5个实体门，
	// 一共增加了43个tile的修改。地图的cell尺寸 0.2*0.2m
	for (int i = 0;i<10;i++)
		m_tileCache->update(0.1f, m_navMesh);
}

void TempObstaclesNavMesh::AddCylinderTO(std::string tag, float& posX, float& posY, float& posZ, float& radius, float& height)
{

	const float p[3] = {posX,posY,posZ};
	dtObstacleRef result ;
	dtStatus ok = m_tileCache->addObstacle(p, radius, height, &result);
	if (dtStatusFailed(ok))
	{
		printf("TempObstaclesNavMesh::AddCylinderTO fail \n");
		return;
	}

	std::map<std::string, std::vector<dtObstacleRef>>::iterator it = m_mapTag2TO.find(tag);
	if (it == m_mapTag2TO.end())
	{
		std::vector<dtObstacleRef> tmp;
		m_mapTag2TO[tag] = tmp;
	}

	std::vector<dtObstacleRef>& curStun = m_mapTag2TO[tag];
	curStun.push_back(result);
	//printf("TempObstaclesNavMesh::AddCylinderTO result: %u\n", result);

#ifdef OUTPUT_DEBUG
	ofs << GetTimestamp() << " TempObstaclesNavMesh::AddCylinderTO " << this <<" tag " << tag
		<< " cent " << posX << "," << posY << "," << posZ
		<< " radius " << radius << " height " << height << std::endl;
#endif
}

void TempObstaclesNavMesh::AddObbBox( std::string tag, float& centX, float& centY, float& centZ, float& halfX, float& halfY, float& halfZ, float& rotaY)
{
	// OBB当旋转角度在这些区间（[-5°,5°] [175,-175°] [85°,95°] [-95°,-85°]），将会强制纠成AABB；
	static const auto PER = M_PI / 180.0f;
	if (((rotaY >= -5.0f * PER) && (rotaY <= 5.0f * PER))
		|| ((rotaY >= 175.0f* PER) || (rotaY <= -175.0f*PER)))
	{
		float minPos[3] = { centX - halfX,centY - halfY, centZ - halfZ };
		float maxPos[3] = { centX + halfX,centY + halfY, centZ + halfZ };
		AddAabbBox(tag, minPos[0], minPos[1], minPos[2], maxPos[0], maxPos[1], maxPos[2]);
		return;
	}
	else if (((rotaY >= 85.0f * PER) && (rotaY <= 95.0f * PER))
		|| ((rotaY >= -95.0f * PER) && (rotaY <= -85.0f * PER)))
	{
		float minPos[3] = { centX - halfZ,centY - halfY, centZ - halfX };
		float maxPos[3] = { centX + halfZ,centY + halfY, centZ + halfX };
		AddAabbBox(tag, minPos[0], minPos[1], minPos[2], maxPos[0], maxPos[1], maxPos[2]);
		return;
	}


	const float center[3] = { centX,centY,centZ };
	const float halfExtents[3] = { halfX,halfY,halfZ };
	dtObstacleRef result;
	dtStatus ok = m_tileCache->addBoxObstacle(center, halfExtents, rotaY, &result);
	if (dtStatusFailed(ok))
	{
		// printf("TempObstaclesNavMesh::AddObbBox addBoxObstacle fail \n");
#ifdef OUTPUT_DEBUG
		ofs << GetTimestamp() << " TempObstaclesNavMesh::AddObbBox " << this << " fail tag " << tag
			<< " cent " << centX << "," << centY << "," << centZ
			<< " half " << halfX << "," << halfY << "," << halfZ
			<< " rot " << rotaY << std::endl;
#endif
		return;
	}

	std::map<std::string, std::vector<dtObstacleRef>>::iterator it = m_mapTag2TO.find(tag);
	if (it == m_mapTag2TO.end())
	{
		std::vector<dtObstacleRef> tmp;
		m_mapTag2TO[tag] = tmp;
	}

	std::vector<dtObstacleRef>& curStun = m_mapTag2TO[tag];
	curStun.push_back(result);
#ifdef OUTPUT_DEBUG
	ofs << GetTimestamp() << " TempObstaclesNavMesh::AddObbBox "<< this <<" tag " << tag
		<< " cent " << centX << "," << centY << "," << centZ
		<< " half " << halfX << "," << halfY << "," << halfZ
		<< " rot " << rotaY << std::endl;
#endif
}

void TempObstaclesNavMesh::AddAabbBox(std::string tag, float& minX, float& minY, float& minZ, float& maxX, float& maxY, float& maxZ)
{
	const float minp[3] = { minX,minY,minZ };
	const float maxp[3] = { maxX,maxY,maxZ };
	dtObstacleRef result;
	dtStatus ok = m_tileCache->addBoxObstacle(minp, maxp,&result);
	if (dtStatusFailed(ok))
	{
		return;
	}

	std::map<std::string, std::vector<dtObstacleRef>>::iterator it = m_mapTag2TO.find(tag);
	if (it == m_mapTag2TO.end())
	{
		std::vector<dtObstacleRef> tmp;
		m_mapTag2TO[tag] = tmp;
	}

	std::vector<dtObstacleRef>& curStun = m_mapTag2TO[tag];
	curStun.push_back(result);
}

void TempObstaclesNavMesh::AddObstacle(float* p)
{
	m_tileCache->addObstacle(p, 1.0f, 2.0f, 0);
}

void TempObstaclesNavMesh::AddBoxObstacle(float minPos[3], float maxPos[3])
{
	auto result = m_tileCache->addBoxObstacle(minPos, maxPos, 0);
	if (dtStatusFailed(result))
	{
		printf("TempObstaclesNavMesh::AddBoxObstacle failed\n");
	}
	else
	{
		// printf("TempObstaclesNavMesh::AddBoxObstacle success\n");
	}
}


void TempObstaclesNavMesh::RemoveOneObstacle(int idx)
{
	if (!m_tileCache) {
		return;
	}

	if (idx >= m_tileCache->getObstacleCount()) {
		printf("TempObstaclesNavMesh::RemoveOneObstacle Too Large Idx!\n");
		return;
	}

	const dtTileCacheObstacle* ob = m_tileCache->getObstacle(idx);
	if (ob->state == DT_OBSTACLE_EMPTY) {
		return;
	}

	m_tileCache->removeObstacle(m_tileCache->getObstacleRef(ob));
}

void TempObstaclesNavMesh::RemoveAllObstacle()
{
	if (!m_tileCache)
		return;
	for (int i = 0; i < m_tileCache->getObstacleCount(); ++i)
	{
		const dtTileCacheObstacle* ob = m_tileCache->getObstacle(i);
		if (ob->state == DT_OBSTACLE_EMPTY) continue;
		m_tileCache->removeObstacle(m_tileCache->getObstacleRef(ob));
	}
}


void TempObstaclesNavMesh::RemoveTOByTag(std::string tag)
{
#ifdef OUTPUT_DEBUG
	ofs << GetTimestamp() << " TempObstaclesNavMesh::RemoveTOByTag " << this
<< " tag " << tag << std::endl;
#endif
	const dtTileCacheObstacle* ob = 0;
	std::map<std::string, std::vector<dtObstacleRef>>::iterator it = m_mapTag2TO.find(tag);
	if (it == m_mapTag2TO.end())
	{
		printf("TempObstaclesNavMesh::RemoveTOByTag fail, tag not find %s\n", tag.c_str());
		return;
	}
	std::vector<dtObstacleRef>& curStun = it->second;
	for (int i =0; i < curStun.size();i++)
	{
		ob = m_tileCache->getObstacleByRef(curStun[i]);
		if (ob == nullptr)
		{
			printf("TempObstaclesNavMesh::RemoveTOByTag warning, not find cache obstacle ref: %u\n", curStun[i]);
		}
		else if (ob->state == DT_OBSTACLE_EMPTY)
		{
			printf("TempObstaclesNavMesh::RemoveTOByTag warning, not find cache obstacle ref: %u\n", curStun[i]);
		}
		else
		{
			m_tileCache->removeObstacle(m_tileCache->getObstacleRef(ob));
			// printf("TempObstaclesNavMesh::RemoveTOByTag success, cache obstacle ref: %u\n", curStun[i]);
		}
	}
	curStun.clear();
}

void TempObstaclesNavMesh::RemoveAllTO()
{
	const dtTileCacheObstacle* ob = 0;
	for (auto&& it : m_mapTag2TO)
	{
		std::vector<dtObstacleRef>& curStun = it.second;
		for (int i = 0; i < curStun.size(); i++)
		{
			ob = m_tileCache->getObstacleByRef(curStun[i]);
			if (ob == nullptr)
			{
				printf("TempObstaclesNavMesh::RemoveAllTO warning, not find cache obstacle ref: %u\n", curStun[i]);
				continue;
			}
			else if (ob->state == DT_OBSTACLE_EMPTY)
			{
				printf("TempObstaclesNavMesh::RemoveAllTO warning, not find cache obstacle ref: %u\n", curStun[i]);
			}
			else
			{
				m_tileCache->removeObstacle(m_tileCache->getObstacleRef(ob));
				// printf("TempObstaclesNavMesh::RemoveAllTO success, cache obstacle ref: %u\n", curStun[i]);
			}

		}
		curStun.clear();
	}
	m_mapTag2TO.clear();
}
void TempObstaclesNavMesh::Reset()
{
	m_nstraightPath = 0;
	m_npolys = 0;
	m_nearCount = 0;
	m_hitCount = 0;
}
bool TempObstaclesNavMesh::Find(float& sposX, float& sposY, float& sposZ, float& eposX, float& eposY, float& eposZ)
{
	Reset();
	m_startRef = 0;
	m_endRef = 0;

	m_spos[0] = sposX;
	m_spos[1] = sposY;
	m_spos[2] = sposZ;

	m_epos[0] = eposX;
	m_epos[1] = eposY;
	m_epos[2] = eposZ;

	auto sFindResult = m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
	if (!dtStatusSucceed(sFindResult))
	{
		printf("TempObstaclesNavMesh::Find fail, %ud\n", sFindResult);
		return false;
	}
	auto eFindResult = m_navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, 0);
	if (!dtStatusSucceed(eFindResult))
	{
		printf("TempObstaclesNavMesh::Find fail, %ud\n", eFindResult);
		return false;
	}

	auto findResult = m_navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);

	if (!dtStatusSucceed(findResult))
	{
		return false;
	}

	if (m_npolys)
	{

		float epos[3];
		dtVcopy(epos, m_epos);
		if (m_polys[m_npolys - 1] != m_endRef)
			m_navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos, 0);

		m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
			m_Path, m_PathFlags,
			m_PathPolys, &m_nstraightPath, MAX_POLYS, 0);
		if (m_nstraightPath)
		{
			return true;
		}
	}
	return false;
}

float* TempObstaclesNavMesh::GetPath(int* count)
{
	*count = m_nstraightPath;
	return m_Path;
}

bool TempObstaclesNavMesh::CalcNear(float& sposX, float& sposY, float& sposZ)
{

	if (!m_navMesh)
	{
		printf("TempObstaclesNavMesh::CalcNear m_navMesh is null! \n");
		m_nearCount = 0;
		return false;
	}

	dtPolyRef m_startRef = 0;
	dtPolyRef m_endRef = 0;
	float m_spos[3];
	m_spos[0] = sposX;
	m_spos[1] = sposY;
	m_spos[2] = sposZ;

	dtStatus status = m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, m_nearPos);

	if (m_startRef)
	{

		m_nearCount = 1;
		return true;
	}
	else {
		m_nearCount = 0;
	}

	return false;
}

float* TempObstaclesNavMesh::GetNear(int* count)
{
	*count = m_nearCount;
	return m_nearPos;
}

bool TempObstaclesNavMesh::CalcRaycast(float& sposX, float& sposY, float& sposZ, float& eposX, float& eposY, float& eposZ)
{
//#ifdef OUTPUT_DEBUG
//	ofs << GetTimestamp() << " TempObstaclesNavMesh::CalcRaycast " << this << " "
//		<< " spos: " << sposX << "," << sposY << "," << sposZ << " "
//		<< " epos: " << eposX << "," << eposY << "," << eposZ << " "
//		<< std::endl;
//#endif
	m_hitResult = false;
	m_hitCount = 0;

	if (!m_navMesh)
	{
		printf("TempObstaclesNavMesh::CalcRaycast m_navMesh is null! \n");
		return m_hitResult;
	}

	dtPolyRef m_startRef = 0;
	float m_spos[3] = { sposX ,sposY,sposZ };
	float m_epos[3] = { eposX ,eposY,eposZ };
	float t = 0;
	float m_hitNormal[3];

	dtStatus status = 0;

	status = m_navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, 0);
	if (dtStatusFailed(status)) {
		return false;
	}
	status = m_navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, m_hitNormal, m_polys, &m_npolys, MAX_POLYS);
	if (dtStatusFailed(status)) {
		return false;
	}
	if (t > 1)
	{
		// No hit
		dtVcopy(m_hitPos, m_epos);
		m_hitResult = false;
		m_hitCount = 0;
//#ifdef OUTPUT_DEBUG
//		ofs << GetTimestamp() << " TempObstaclesNavMesh::CalcRaycast " << this << " no hit " << std::endl;
//#endif
	}
	else
	{
		// Hit
		dtVlerp(m_hitPos, m_spos, m_epos, t);
		m_hitResult = true;
		m_hitCount = 1;
//#ifdef OUTPUT_DEBUG
//		ofs << GetTimestamp() << " TempObstaclesNavMesh::CalcRaycast " << this << " hit "
//			<< " m_hitPos: " << m_hitPos[0] << "," << m_hitPos[1] << "," << m_hitPos[0] << std::endl;
//#endif
	}
	return m_hitResult;
}

float* TempObstaclesNavMesh::GetRaycast(int* count)
{
	*count = m_hitCount;
	return m_hitPos;
}
